/***************************************************************************
 *  Filename: maincontroller.cpp
 *  Class: MainController
 *  
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *  
 ****************************************************************************/


#include "maincontroller.h"
#include <QDebug>

MainController::MainController(QObject *parent) :
    QObject(parent)
{
    mutex.lock();
    targetX = 0;
    targetY = 0;
    targetZ = 0.5;
    mutex.unlock();
    connect(&timer, SIGNAL(timeout()), this, SLOT(controlLoop()));

}

void MainController::updateAnglesData(float troll, float tpitch, float tyaw){
    mutex.lock();
    roll = troll;
    pitch = tpitch;
    yaw = tyaw;
    mutex.unlock();
}

void MainController::updateRelativeTarget(float xt, float yt, float zt, float yawt){
    mutex.lock();
    targetX = xt;
    targetY = yt;
    targetZ = zt;
    targetYaw = yawt;
    mutex.unlock();
}

void MainController::updateSpeeds(float tvx, float tvy, float tvz){
    qDebug() << "[MainController] New velocity values received" << tvx << tvy << tvz;
    mutex.lock();
    vx = tvx/1000.0;
    vy = tvy/1000.0;
    vz = tvz/1000.0;
    float time = speedIntegrationTime.elapsed()/1000.0;
    speedIntegrationTime.restart();

    targetX -= vx*time;
    targetY -= vy*time;
    qDebug() << "[MainController] Diff: " << time << targetX << targetY;
    mutex.unlock();
}

void MainController::updateAltitude(float taltitude){
    mutex.lock();
    double time = speedDerivationTime.elapsed()/1000.0;
    //(altitude-prev_alt)/time equals speed.
    vz = (taltitude - altitude)/time;
    altitude = taltitude;
    speedDerivationTime.restart();
    mutex.unlock();
}

void MainController::controlLoop(){
    mutex.lock();
    float taltitude = altitude;

    float troll = roll;
    float tyaw = yaw;
    float tpitch = pitch;

    float tVx = vx;
    float tVy = vy;
    float tVz = vz;

    float tTx = targetX;
    float tTy = targetY;
    float tTz = targetZ;
    float tTyaw = targetYaw;

    mutex.unlock();

    float desiredPitch = ((tTx)*Ppp + tVx*Dpp + tpitch*Ddp)/MaxPitch;
    qDebug() << "[MainController] controlLoop tTx="<<tTx<<"tVx="<<tVx<<"tpitch="<<tpitch;
    float desiredRoll = ((tTy)*Ppr + tVy*Dpr + troll*Ddr)/MaxRoll;
    qDebug() << "[MainController] controlLoop tTy="<<tTy<<"tVy="<<tVy<<"troll="<<troll;
    float desiredVz = (tTz + taltitude*Psg + tVz*Dsg)/MaxGaz;
    qDebug() << "[MainController] controlLoop tTz="<<tTz<<"taltitude="<<taltitude<<"tVz="<<tVz;
    float desiredYawRate = (tTyaw + tyaw*Py)/MaxYaw;
    qDebug() << "[MainController] controlLoop tTyaw="<<tTyaw<<"tyaw="<<tyaw;
    emit(updateCommands(desiredRoll, desiredPitch, desiredYawRate, desiredVz));
}
